#include "geometry.hpp"
#include "topology.hpp"
#include "edges.hpp"
#include "edge.hpp"
#include "mesh.hpp"
#include <cmath> // for fabs() and copysign()

#include <iostream>

namespace hms
{


/**
 * @brief Calculates a pseudoangle between a (difference) vector and the x-axis,
 * which can be used to sort vectors by angle. Has same monotonicity as atan2,
 * but is much faster. Source: https://stackoverflow.com/a/16542043
 * @param dif (Difference) vector between origin and 2D-point
 * @return a number in [-2,2], which is monotonic in the angle between input
 * vector and x-axis and with the same discontinuity as atan2
 * for reference, here are the C implementations of the pseudoangle
 * double pang1 ( double dx, double dy ){
 *   double p = dx/(fabs(dx)+fabs(dy));
 *   if (dy < 0)
 *     return p - 1;
 *   else
 *     return 1 - p;
 * }
 * 
 * double pang2 ( double dx, double dy ){
 *   return copysign(1. - dx/(fabs(dx)+fabs(dy)),dy);
 * }
 *
 */
double  pseudoangle( const Ref<const Vector2s>& dif )
{
	/* with Eigen methods */
	return copysign(1. - dif[xdim]/dif.cwiseAbs().sum(), dif[ydim]);
	/* without Eigen methods */
	return copysign(1. - dif[xdim]/(fabs(dif[xdim])+fabs(dif[ydim])),dif[ydim]);
}

double pseudoangle( const Mesh& mesh, Index vertex){
	return pseudoangle( mesh.vertex(vertex) );
}


/**
 * @brief Calculates the row-wise average of 2D vectors, such as vertices.
 * Is intended to be used for the calculation of cell centroids.
 * @param m matrix whose columns are the 2D vectors to be averaged
 * @return a 2D vector that holds the row-wise average of the passed in matrix.
 */
Vector2s vectorAverage( const Ref<const Matrix2Xs>& m ) {
	return m.rowwise().mean();
}


/**
 * @brief There are two possible centroid points for a square created from an
 * edge AB: one to the left and one to the right of the edge.
 * Those centroid points lie halfway between A and B and the same
 * distance to either Side. Let D=B-A, then
 * @code
 *                          ( 0  -1 )
 * C1,2 = A + 1/2 * ( D +/- ( 1   0 ) * D )
 * @endcode
 * Of the two returned points, the first one is always to the left,
 * the second one always to the right of the edge.
 * @param start Starting point of the edge
 * @param end End point of the edge
 * @return 2x2 matrix, whose column vectors are the two square centroids.
 */
Matrix2s  edgeToSquareCenters(
	const Ref<const Vector2s>& start,
	const Ref<const Vector2s>& end )
{
	const Vector2s edge { end-start };
	
	Matrix2s m;
	/* the above equation shows that both centroids have the A + 0.5 * D part,
	 * so it's broadcasted into the matrix
	 */
	m.colwise() = start + 0.5*edge;
	/*                               ( 0  -1 )
	 * the missing part now is 0.5 * ( 1   0 ) * ( D  -D )
	 */
	return m + 0.5 *
		(Matrix2s{} <<
			0, -1, 
			1,  0).finished() *
		(Matrix2s{} << edge, -edge).finished();
}

Matrix2s edgeToSquareCenters(
	const Mesh& mesh,
	Index edge
){
	return edgeToSquareCenters(
		mesh.vertex(mesh.edges().vertices(edge)[0]),
		mesh.vertex(mesh.edges().vertices(edge)[1])
	);
}


/**
 * @brief Uses shoelace formula to calculate the area of a closed polygon
 * @param m corners of polygon. Have to be sorted counter-clockwise.
 * @return area of polygon
 */
scalar verticesToArea( const Ref<const Matrix2Xs>& m ) {
	/* shoelace formula (Gausssche Trapezformel) */
	scalar area {0};
	
	/* shoelace formula is for closed loops, so we need to loop around to
	 * index 0 at the end */
	for ( Index i {0}; i < m.cols() ; ++i )
	{
		area += (
			m.col(i) +
			m.col(i == m.cols()-1 ? 0 : i+1 ).cwiseProduct(Vector2s{-1, 1})
		).prod();
	}
	
	return area/2;
}




/**
 * @brief Calculates the centroid of a polygon. If the area is not provided, 
 * calls verticesToArea()
 * 
 * @param m the corners of the polygon
 * @param area Area of the polygon. Optional, but faster if provided.
 * @return a 2D vector holding the centroid coordinates
 * 
 * The centroid coordinates Cx and Cy of a polygon are calculated with
 * @code
 * Cx = 1/(6A) * sum( (x[i] + x[i+1]) * (x[i]*y[i+1] - x[i+1]*y[i]) )
 * Cy = 1/(6A) * sum( (y[i] + y[i+1]) * (x[i]*y[i+1] - x[i+1]*y[i]) ),
 * @endcode
 * with A being the area and the index i looping around to zero at the end.
 * If the passed-in polygon is a triangle, the centroid is equal to the 
 * average of the coordinates. Then, only the simplified calculation is carried
 * out.
 */
Vector2s verticesToCentroid( const Ref<const Matrix2Xs>& m, scalar area ){
	/* for a triangle, the centroid is simply the average */
	if ( m.cols() == 3 ) return vectorAverage(m);
	
	/* if no area was provided, calculate it first */
	if ( area==0 ) area = verticesToArea(m);
	
	/* initialise centroid to zeros, as it is added to later */
	Vector2s centroid {0,0};
	
	/* (x[i]*y[i+1] - x[i+1]*y[i]) ) is
	 * col(i).dot( (col(i+1).cwiseProduct({-1, 1})).reverse() )
	 */
	for ( Index i {0}; i < m.cols() ; ++i )
	{
		Index next_i { i == m.cols()-1 ? 0 : i+1 };
		
		centroid +=
			( m.col(i) + m.col(next_i) ) *
			m.col(i).dot(
				( m.col(next_i).cwiseProduct(Vector2s{-1, 1}) ).reverse()
			);
	}
	
	return (1 / ( 6*area) ) * centroid;
}

/**
 * @brief Returns true iff the two line segments AB and CD intersect
 * 
 * @param AB contains points A and B as columns
 * @param CD contains points C and D as columns
 */
bool intersects( const Ref<const Matrix2s>& AB, const Ref<const Matrix2s>& CD ){

	auto CD_line { ParamLine::Through(CD.col(0), CD.col(1)) };
	scalar CD_intersecParam { CD_line.intersectionParameter( Line::Through(AB.col(0), AB.col(1)) ) };
	Vector2s point { CD_line.pointAt(CD_intersecParam) };
	return
		   boundingBox(AB).contains( point )  // is on A-B
		&& boundingBox(CD).contains( point ); // is on C-D
}

/**
 * @brief Returns true if the line segment A and B intersects with the
 * line segment CD. 
 */
bool intersects(
	const Ref<const Vector2s>& A,
	const Ref<const Vector2s>& B,
	const Ref<const Vector2s>& C,
	const Ref<const Vector2s>& D
){
	Matrix2s AB, CD;
	AB << A, B;
	CD << C, D;
	return intersects(AB, CD);
	// auto CD { ParamLine::Through(C,D) };
	// scalar CD_intersecParam { CD.intersectionParameter( Line::Through(A,B) ) };
	// Vector2s point { CD.pointAt(CD_intersecParam) };
	// return
	// 	   boundingBox(AB_m).contains( point )  // is on A-B
	// 	&& boundingBox(CD_m).contains( point ); // is on C-D
}

/**
 * @brief Mirrors a point along an Eigen::ParametrizedLine.
 * 
 * @param line The line along which the point is to be mirrored.
 * @param p The point to be mirrored.
 * @return Vector2s The mirrored point.
 */
Vector2s mirrorAlongLine( const ParamLine& line, const Ref<const Vector2s>& p){
	return p + 2*(line.projection(p) - p);
}


void transformCoords(
	Ref<Array2Xs> transformed,
	const Ref<const Array2Xs>& cartesian,
	const Ref<const Array2Xs>& normals
){
	assert( transformed.cols() == cartesian.cols() );
	assert( cartesian.cols() == normals.cols() );

	/* let each normal vector n be (nx, ny), and the transformed vector v be 
	 * v_T, then v_T = T * v, with the transformation matrix T being
	 * |  nx, ny |
	 * | -ny, nx | */
	/* so the first row is a colwise dot product */
	transformed.row(0) = (normals * cartesian).colwise().sum();
	/* and for the second row, the row orders and one sign have to be reversed */
	transformed.row(1) =
		  normals.row(0) * cartesian.row(1)
		- normals.row(1) * cartesian.row(0);
}

/** @brief Transforms coordinates of vector v into those provided by the normal
 * vector (i.e. first component will be in the normal direction, second in the
 * orthogonal one). This operation is carried out for arrays of vectors and normal
 * vectors
 * 
 * Creates a rotational matrix from the normal vector and applies it to v */
Array2Xs transformCoords(
	const Ref<const Array2Xs>& v,
	const Ref<const Array2Xs>& normals
){
	Array2Xs transformed (2, v.cols() );
	transformCoords(transformed, v, normals);
	return transformed;
}

/** @brief Transforms coordinates of vector v into those provided by the normal
 * vector (i.e. first component will be in the normal direction, second in the
 * orthogonal one). This operation is carried out for arrays of vectors and normal
 * vectors. Instead of returning a transformed vector, the input vector 
 * is transformed.
 * 
 * Creates a rotational matrix from the normal vector and applies it to v */
void transformCoordsInPlace(
	Ref<Array2Xs> v,
	const Ref<const Array2Xs>& normals
){
	/* prevent aliasing */
	Array2Xs temp { transformCoords(v, normals) };
	v = std::move(temp);
}

void retransformCoords(
	Ref<Array2Xs> cartesian,
	const Ref<const Array2Xs>& transformed,
	const Ref<const Array2Xs>& normals
){
	assert( transformed.cols() == cartesian.cols() );
	assert( cartesian.cols() == normals.cols() );

	/* let each normal vector n be (nx, ny), and the transformed vector v be 
	 * v_T, then v_T = T * v, with the transformation matrix T being
	 * | nx, -ny |
	 * | ny,  nx | */
	cartesian.row(0) =
		  normals.row(0) * transformed.row(0)
		- normals.row(1) * transformed.row(1);
	cartesian.row(1) =
		  normals.row(1) * transformed.row(0)
		+ normals.row(0) * transformed.row(1);
}

/** @brief Retransforms coordinates of transformed vector v back into 
 * those of the original euclidean coordinate system.
 * This operation is carried out for arrays of vectors and normal vectors.
 */
Array2Xs retransformCoords(
	const Ref<const Array2Xs>& transformed,
	const Ref<const Array2Xs>& normals
){
	Array2Xs cartesian (2, transformed.cols() );
	retransformCoords(cartesian, transformed, normals);
	return cartesian;
}

/** @brief Retransforms coordinates of transformed vector v into 
 * those of the coordinate system. This operation is carried out 
 * for arrays of vectors and normal vectors.
 * Instead of returning a transformed vector, the input velocity vector 
 * is transformed.
 */
void retransformCoordsInPlace(
	Ref<Array2Xs> transformed,
	const Ref<const Array2Xs>& normals
){
	/* prevent aliasing */
	Array2Xs temp { retransformCoords(transformed, normals) };
	transformed = std::move(temp);
}

} // end namespace hms